#include "mex.h"
#define MODE (20)
#define PI				(3.141592653589793)

#define TAB_D_LENG      (4)
const double	TAB_D_DEP_RATE[TAB_D_LENG]={0.10,0.20,0.50,1.00};
const double	TAB_D_K[TAB_D_LENG]={0.9962,0.9914,0.9730,0.9532};
const double	TAB_D_TAU[TAB_D_LENG]={10.0740,5.3215,2.1915,1.1525};

// New data (2009.08.14 by Gangshi)
// #define	TAB_D_LENG		(10)
// const double	TAB_D_DEP_RATE[TAB_D_LENG]={0.10,0.15,0.20,0.25,0.30,0.35,0.40,0.45,0.50,1.00};
// const double	TAB_D_K[TAB_D_LENG]={0.9962,0.9940,0.9914,0.9882,0.9849,0.9819,0.9791,0.9757,0.9730,0.9532};
// const double	TAB_D_TAU[TAB_D_LENG]={10.0740,7.0783,5.3215,4.2761,3.6503,3.1428,2.7079,2.4471,2.1915,1.1525};

// Constants/model parameters for roughness
#define	TAB_R_LENG		(5)
const double	TAB_R_DEP_RATE[TAB_R_LENG]={0.10,0.15,0.20,0.50,1.00};
const double	TAB_R_NU[TAB_R_LENG]={0.0451e-4,1.3915e-005,0.3583e-4,0.1232e-3,0.3939e-3};
const double	TAB_R_SIGMA2[TAB_R_LENG]={1.0810e-3,6.2341e-3,2.1983e-2,0.0990,0.3007};
const double	TAB_R_RH[TAB_R_LENG]={0.1002,0.1509,0.2021,0.5129,1.0432};

struct MPCSettings
{
	double	D_set;
	double	Fact_D;
	double	R2_set;	 // layer^2
	double	Fact_r2;
	double	Ht_set;		 // layer
	double	Fact_H;
	int		Rel_err;
//	double	DepRate_ini; // layer/s
	double  dt;
	int		P;
};

// ------------------------------------------------------------------
void para_r_fit(double DEP_RATE,double *K,double *tau,double *nu,double *sigma2,double *rh) 
// fit the parameters from experimental data using linear piecewise interpolation 
{
	int i;
	double factor;

	// K and tau from TAB_D
	for(i=0;i<TAB_D_LENG-1;i++)
	{
		if((DEP_RATE>TAB_D_DEP_RATE[i])&&(DEP_RATE<=TAB_D_DEP_RATE[i+1]))
		{
			factor=(DEP_RATE-TAB_D_DEP_RATE[i])/(TAB_D_DEP_RATE[i+1]-TAB_D_DEP_RATE[i]);
			/*
			mexPrintf("factor = %f\n",factor);
			mexPrintf("DEP_RATE-TAB_D_DEP_RATE[%d] = %f\n", i, DEP_RATE-TAB_D_DEP_RATE[i]);
			mexPrintf("DEP_RATE = %f, TAB_D_DEP_RATE[%d] = %f\n", DEP_RATE, i, TAB_D_DEP_RATE[i]);
			mexPrintf("TAB_D_DEP_RATE[%d+1]-TAB_D_DEP_RATE[%d] = %f\n", i,i,TAB_D_DEP_RATE[i+1]-TAB_D_DEP_RATE[i]);
			*/
			*K=TAB_D_K[i]+(TAB_D_K[i+1]-TAB_D_K[i])*factor;
			*tau=TAB_D_TAU[i]+(TAB_D_TAU[i+1]-TAB_D_TAU[i])*factor;
		}
	}

	if(DEP_RATE<=TAB_D_DEP_RATE[0])
	{
		*K=TAB_D_K[0];
		*tau=TAB_D_TAU[0];
	}

	if(DEP_RATE>TAB_D_DEP_RATE[TAB_D_LENG-1])
	{
		*K=TAB_D_K[TAB_D_LENG-1];
		*tau=TAB_D_TAU[TAB_D_LENG-1];
	}

	// nu, sigma2 and rh from TAB_R
	for(i=0;i<TAB_R_LENG-1;i++)
	{
		if((DEP_RATE>TAB_R_DEP_RATE[i])&&(DEP_RATE<=TAB_R_DEP_RATE[i+1]))
		{
			factor=(DEP_RATE-TAB_R_DEP_RATE[i])/(TAB_R_DEP_RATE[i+1]-TAB_R_DEP_RATE[i]);
			// mexPrintf("For roughness, factor = %f\n",factor);
			*nu=TAB_R_NU[i]+(TAB_R_NU[i+1]-TAB_R_NU[i])*factor;
			*sigma2=TAB_R_SIGMA2[i]+(TAB_R_SIGMA2[i+1]-TAB_R_SIGMA2[i])*factor;
			*rh=TAB_R_RH[i]+(TAB_R_RH[i+1]-TAB_R_RH[i])*factor;
		}
	}

	if(DEP_RATE<=TAB_R_DEP_RATE[0])
	{
		*nu=TAB_R_NU[0];
		*sigma2=TAB_R_SIGMA2[0];
		*rh=TAB_R_RH[0];
	}

	if(DEP_RATE>TAB_R_DEP_RATE[TAB_R_LENG-1])
	{
		*nu=TAB_R_NU[TAB_R_LENG-1];
		*sigma2=TAB_R_SIGMA2[TAB_R_LENG-1];
		*rh=TAB_R_RH[TAB_R_LENG-1];
	}

	return;
}
// ------------------------------------------------------------------
void objfunc(double *cost, double DEP_RATE[], double alphas[], double betas[], double ht, double g_dens, struct MPCSettings Settings) 
{
	int		i,j;
	double	n;
	int		step_num;
	double	K,tau;
	double	nu,sigma2,rh;
	double	lambda,phi2_ss;	// tau is time constant, phi2_ss is the steady-state value of alpha^2
	double	dens_curr,ht_curr;		// values before predictions
	double	dens_pred,rough2_pred,ht_pred;		// sum of predictions at prediction horizons
	double	var0=0.0;
	double	dev_total=0,dev_r,dev_d,dev_h;
	double	alpha2_pred[MODE],beta2_pred[MODE];		// for cost_funct use only
	double	alpha2_curr[MODE],beta2_curr[MODE];		// for cost_funct use only

	step_num=Settings.P;						

	// assign current states to the predicting variables
	for(j=0;j<MODE;j++)
	{
		alpha2_curr[j]	= alphas[j]*alphas[j];
		beta2_curr[j]	= betas[j]*betas[j];
	}
	ht_curr=ht;
	dens_curr=g_dens;

	dev_total=0.0;
	for(i=1;i<=step_num;i++)
	{
		// get rh, nu and sigma2 for the given DEP_RATE
		// get K and tau for the given DEP_RATE
		para_r_fit(DEP_RATE[i-1],&K,&tau,&nu,&sigma2,&rh);
		//mexPrintf("K = %f\ntau = %f\n nu = %f\nsigma2 = %f\nrh = %f\n",K,tau,nu,sigma2,rh);
		// roughness calculation
		rough2_pred=0.0;
		
		for(j=0;j<MODE;j++)	
		{
			n=j+1;	// the zero-th states are not counted
			lambda=nu*n*n;
			phi2_ss=sigma2/2/lambda;
			alpha2_pred[j]=phi2_ss+(alpha2_curr[j]-phi2_ss)*exp(-Settings.dt*2*lambda);
			beta2_pred[j]=phi2_ss+(beta2_curr[j]-phi2_ss)*exp(-Settings.dt*2*lambda);
			rough2_pred+=alpha2_pred[j]+beta2_pred[j];					// eq(24)
		}
		rough2_pred/=2*PI;
		// calculate the error from the set point
		dev_r=rough2_pred-Settings.R2_set;
		if (Settings.Rel_err)
		{
			dev_r/=Settings.R2_set;	// relative error
		}
		dev_r*=dev_r;
		//mexPrintf("((r2-r2_set)/r2_set)^2 = %f\n",dev_r);
		
		// thickness calculation
		ht_pred=ht_curr+Settings.dt*rh;
		// if the predicted thickness is less than the set-point, add the deviation square to the cost
		// otherwise do nothing, i.e., penalizing on the under-growth film
		if(ht_pred<Settings.Ht_set)	
		{
			dev_h=ht_pred-Settings.Ht_set;
		}
		else
		{
			dev_h=0;
		}
		if (Settings.Rel_err)
		{
			dev_h/=Settings.Ht_set;	// relative error
		}
		dev_h*=dev_h;
		//mexPrintf("((h-h_set)/h_set)^2 = %f\n",dev_h);
		
		// SOR calculation 
		//dens_curr=n_lyr_depos/ht_curr;
		dens_pred=(dens_curr*ht_curr+rh*(K*Settings.dt+(K-dens_curr)*tau*(exp(-Settings.dt/tau)-1)))/(ht_curr+Settings.dt*rh);
		dev_d=dens_pred-Settings.D_set;
		if (Settings.Rel_err)
		{
			dev_d/=Settings.D_set;	// relative error
		}
		dev_d*=dev_d;
		//mexPrintf("((SOR-SOR_set)/SOR_set)^2 = %f\n",dev_d);
		
		//mexPrintf("Step = %d, Cost = %f\n",i,Settings.Fact_D*dev_d+Settings.Fact_r2*dev_r+Settings.Fact_H*dev_h);
		dev_total+=Settings.Fact_D*dev_d+Settings.Fact_r2*dev_r+Settings.Fact_H*dev_h;
		
		// update current states to the predicted variables
		for(j=0;j<MODE;j++)
		{
			alpha2_curr[j]=alpha2_pred[j];
			beta2_curr[j]=beta2_pred[j];
		}
		ht_curr=ht_pred;
		dens_curr=dens_pred;
	}
	*cost = dev_total;
	//printf("%f\n",dev_total);
}
// ------------------------------------------------------------------
void mexFunction(
	int nlhs, mxArray *plhs[],
	int nrhs, const mxArray *prhs[])
// The Matlab version interface is F = objfun(x,feedback,settings)
{
	struct MPCSettings Settings;
	double 		*w;
	double 		*F;
	int 		nfields;
	double		*alpha;
	double		*beta;
	double		rho;
	double		h;
	
	mxArray		*tmp;
	double		*ptmp;
	
	if(nrhs!=3)
		mexErrMsgTxt("Three inputs required");
//	if(nlhs!=1)
//		mexErrMsgTxt("One output required");
		
	if(!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]))
		mexErrMsgTxt("Input X must be a vector");
	if(!mxIsStruct(prhs[1]))
		mexErrMsgTxt("Input FEEDBACK must be a struct");
	if(!mxIsStruct(prhs[2]))
		mexErrMsgTxt("Input SETTINGS must be a struct");
		
	w 	= mxGetPr(prhs[0]);
	
	tmp = mxGetField(prhs[1],0,"alpha");
	alpha = mxGetPr(tmp);
	
	tmp = mxGetField(prhs[1],0,"beta");
	beta = mxGetPr(tmp);
	
	tmp = mxGetField(prhs[1],0,"rho");
	ptmp = mxGetPr(tmp);
	rho = (*ptmp);
	
	tmp = mxGetField(prhs[1],0,"h");
	ptmp = mxGetPr(tmp);
	h = (*ptmp);
	
	tmp = mxGetField(prhs[2],0,"D_set");
	ptmp = mxGetPr(tmp);
	Settings.D_set = (*ptmp);
	//mexPrintf("D_set = %f\n",Settings.D_set);
	
	tmp = mxGetField(prhs[2],0,"Fact_D");
	ptmp = mxGetPr(tmp);
	Settings.Fact_D = (*ptmp);
	//mexPrintf("Fact_D = %f\n",Settings.Fact_D);
	
	tmp = mxGetField(prhs[2],0,"R2_set");
	ptmp = mxGetPr(tmp);
	Settings.R2_set = (*ptmp);
	//mexPrintf("R2_set = %f\n",Settings.R2_set);
	
	tmp = mxGetField(prhs[2],0,"Fact_r2");
	ptmp = mxGetPr(tmp);
	Settings.Fact_r2 = (*ptmp);
	//mexPrintf("Fact_r2 = %f\n",Settings.Fact_r2);
	
	tmp = mxGetField(prhs[2],0,"Ht_set");
	ptmp = mxGetPr(tmp);
	Settings.Ht_set = (*ptmp);
	//
	tmp = mxGetField(prhs[2],0,"Fact_H");
	ptmp = mxGetPr(tmp);
	Settings.Fact_H = (*ptmp);
	//mexPrintf("Fact_H = %f\n",Settings.Fact_H);
	//mexPrintf("Ht_set = %f\n",Settings.Ht_set);
	
	tmp = mxGetField(prhs[2],0,"P");
	ptmp = mxGetPr(tmp);
	Settings.P = (int)(*ptmp);
	//mexPrintf("P = %d\n",Settings.P);
	
	tmp = mxGetField(prhs[2],0,"dt");
	ptmp = mxGetPr(tmp);
	Settings.dt = (*ptmp);
	//mexPrintf("dt = %f\n",Settings.dt);
	
	Settings.Rel_err = 1;

	plhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
	F = mxGetPr(plhs[0]);
	
	objfunc(F,w,alpha,beta,h,rho,Settings);
}

